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Abstract  —  For  many  years,  autonomous  underwater  vehicles 
(AUYs)  have  been  developed  and  employed  for  a  myriad  of  tasks. 
Their  ability  to  accurately  collect  and  monitor  oceanic  conditions 
makes  them  a  valuable  asset  for  a  variety  of  naval  missions. 
Deploying  and  recovering  AUVs,  however,  is  currently  largely 
limited  to  surface  vessels  or  swimmers.  The  purpose  of  this  paper 
is  to  demonstrate  that  by  using  a  mathematical  technique  called 
a  direct  method  of  calculus  of  variations,  it  is  possible  for  an 
AUV  to  autonomously  compute  and  execute  a  trajectory  that  will 
allow  for  recovery  by  a  submerged  mobile  recovery  system 
(another  AUY,  submarine,  etc.).  The  algorithm  ensures  that  a 
smooth  trajectory  is  produced  that,  while  not  traditionally 
optimal,  is  realistic  and  still  close  to  the  optimal  solution.  Also, 
using  this  technique  allows  the  trajectory  to  be  computed  very 
rapidly  allowing  it  to  be  recomputed  every  couple  of  seconds  to 
accommodate  sudden  changes,  possible  adjustments  and 
different  disturbances,  and  therefore  to  be  used  in  the  real  life. 

Keywords'.  Marine  control,  Real-time  control,  Optimization.  Unmanned 
systems. 

I. Introduction 

utonomous  Underwater  Vehicles  (AUVs)  have  been  of 
great  interest  to  the  United  States  Navy  for  quite  some 
time.  This  interest  began  in  1994  with  the  Navy  Unmanned 
Underwater  Vehicles  (UUV)  Program  Plan  which  promoted 
research  and  development  for  the  employment  of  AUVs  from 
submarines  for  mine  warfare  (MIW)  and  tactical 
oceanography.  Since  1994  AUV  concepts  of  operations 
(CONOPS)  have  been  proposed  through  the  UUV  Master 
Plan,  the  Small  UUV  Strategic  Plan,  and  Sea  Power  21  [1,2]. 
Further  mission  areas  may  include  Intelligence,  Surveillance, 
and  Reconnaissance  (ISR),  Anti-Submarine  Warfare  (ASW), 
Communication/Navigation  Network  Node  (CN3),  payload 
delivery,  Information  Operations  (10),  Time  Critical  Strike 
(TCS),  barrier  patrol,  and  sea  base  support  [1],  It  has  been 
over  13  years  since  the  original  concept  of  using  AUVs  was 
proposed  and  the  CONOPS  for  AUV  employment  continues 
to  increase. 

Currently  there  are  limited  options  to  autonomously  launch 
and  recover  AUVs  from  surface  vessels  and  submarines.  The 
ability  to  accomplish  this  will  dramatically  increase  the  utility 
of  AUVs.  To  do  so,  the  AUVs  must  have  the  autonomy 
necessary  to  plan  and  execute  non-linear  trajectories  both  to 
and  away  from  the  supporting  vessel. 

Recently  there  has  been  a  demonstration  of  launching  and 
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recovering  of  a  larger  diameter  AUV  through  the  torpedo  tube 
[3].  Instead  of  recovering  to  a  docking  station  aft  of  the  sail  the 
recovery  path  would  be  to  the  recovery  arm  deployed  out  of 
the  torpedo  tube.  That  said,  in  the  future  there  may  be  many 
AUVs  deployed  from  the  submarine,  in  that  case  deploying 
and  recovering  from  the  torpedo  tube  may  not  be  practical. 

Also,  there  has  been  recent  work  on  AUV  to  AUV 
rendezvous  for  enabling  high  speed  communication  [4].  This 
work  uses  optimal  control  theory  for  calculating  time  and 
energy  optimal  solutions  for  the  rendezvous  path. 
Unfortunately,  indirect  method  solutions  cannot  be  computed 
in  real-time  (if  at  all),  and  use  very  simplified  models,  so  they 
are  not  practical  and  flexible  enough  for  a  real-time 
implementation  on  an  AUV  except  under  a  specific,  relatively 
narrow  set  of  conditions. 

In  a  real  life  situation,  however,  the  rendezvous  would  be 
unlikely  to  have  ideal  conditions.  The  supporting  vessel  may 
need  to  maneuver  to  avoid  a  collision,  currents  may  change 
the  approach  geometry,  speed  adjustments  may  need  to  be 
made,  etc.  All  these  changes  could  obviously  jeopardize  an 
ideal  time  variant  solution.  Therefore,  a  method  that  is  not 
time-variant  must  be  pursued.  While  not  necessarily  optimal 
in  the  classical  control  theory  sense,  such  a  solution  should  be 
feasible  and  good  enough  to  allow  for  autonomous  AUV 
recovery,  while  still  taking  into  consideration  the  factors  of 
optimization. 

This  paper  deals  with  employing  the  direct  method  of 
calculus  of  variations  to  generate  rendezvous  trajectories  in 
faster  than  a  real-time  scale.  That  means  that  the  CPU  time 
should  be  of  the  order  of  1%  of  the  maneuver  time.  Direct 
methods  introduced  in  dynamic  optimization  of  the 
trajectories  of  aerospace  vehicles  in  the  1950’s  were  a  very 
robust  way  of  controlling  a  vehicle.  It  assured  all  boundary 
conditions  and  possible  dynamic  constraints  were  satisfied, 
and  provided  a  smooth  path  for  the  entire  trajectory  using  only 
a  few  varied  parameters.  Similarly,  this  methodology  can 
satisfy  both  time  and  speed  constraints  for  the  case  of  AUV 
recovery  fairly  easily,  while  providing  a  near-optimal 
real-time  solution. 

The  paper  is  organized  as  follows.  Section  II  introduces  the 
general  scenario  for  the  development  of  a  path-planning 
methodology,  which  should  generate  trajectories  for  the 
recovery  of  an  AUV  to  a  mobile  docking  station  [5].  Section 
III  addresses  the  AUV  model  needed  to  develop  a  controller. 
Section  IV  introduces  the  key  aspects  of  the  proposed 
approach  to  compute  rendezvous  trajectories  and  explains  the 
factors  affecting  the  shape  of  the  path.  Finally,  Section  V 
presents  some  simulation  results.  The  paper  ends  with 
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conclusions. 

TT.PRom.RM  Formulation 

This  paper  addresses  a  general  scenario  for  autonomous 
recovery  of  the  AUV  by  a  mobile  underwater  recovery  system 
(MURS),  which  can  be  another  AUV,  docking  station  towed 
from  a  surface  ship,  submarine,  etc.  It  is  assumed  that  this 
mission  will  proceed  in  a  several  stages  as  follows: 

-  the  AUV  has  completed  its  mission  and  returns  to  a 
predetermined  loitering  point  at  a  predetermined  time; 

-  the  MURS  sends  a  message  to  the  AUV  suggesting  a 
rendezvous  point  (area)  and  time; 

-  the  AUV  plans  a  trajectory  to  rendezvous  with  the  MURS 
at  a  given  position  and  time  and  sends  an  acknowledgment 
back  to  the  MURS; 

-  the  AUV  executes  the  plan  and  recovers  to  the  MURS. 

-  as  the  AUV  gets  close  to  the  MURS,  final  navigation  to  the 
recovery  platform  is  accomplished  through  a  homing 
transponder. 

With  this  recovery  methodology,  there  are  a  couple  of 
points  worth  expanding  upon.  First,  once  the  AUV  has 
returned  to  the  loitering  point,  the  MURS  must  be  in  an  area  to 
ensure  the  best  chance  of  communicating  with  and  recovering 
the  vehicle.  In  this  paper  we  assume  the  MURS  will  maintain  a 
rectangular  racetrack  (however,  from  the  algorithmic 
standpoint  it  does  not  really  matter,  so  that  a  circular  or  any 
other  track  could  be  used  instead). 

Second,  once  the  vehicle  has  planned  and  started  to  execute 
the  trajectory  to  the  MURS,  the  trajectory  must  be  updatable 
to  handle  disturbances  (unmodeled  dynamics,  currents)  and 
different  unforeseen  events.  These  events  include  the  cases 
when  the  MURS  maneuvers  inadvertently  or  the  AUV  must 
conduct  reactive  obstacle  avoidance  during  the  execution  of 
the  rendezvous  path. 

Third,  it  is  assumed  that  the  preferred  method  for 
recovering  the  AUV  is  for  it  to  approach  from  the  port  or 
starboard  aft  quarter  of  MURS  and  maneuver  to  the  final 
recovery  location.  The  trajectory  of  the  vehicle  must  be  such 
that  it  avoids  running  the  vehicle  into  the  control  or  propulsion 
surfaces  while  the  vehicle  makes  its  approach  to  the  recovery 
device. 

Fourth,  it  is  envisioned  that  a  signal  can  be  transmitted  to 
AUV  that  includes  some  basic  parameters,  such  as  position, 
course,  depth,  and  rendezvous  time,  so  that  the  AUV  could 
autonomously  plan  a  path  to  rendezvous  with  the  MURS  for 
recovery. 

In  this  rendezvous  scenario,  the  MURS  would  establish  a 
race  track,  which  allows  it  to  travel  back  and  forth  along  two 
long  track  legs  (see  Fig.l).  These  legs  are  needed  to  allow 
sufficient  time  to  contact  the  AUV  (which  is  assumed  to  be  in 
its  holding  pattern  somewhere  within  communication  range) 
and  allow  it  to  transit  from  its  holding  pattern  to  the 
rendezvous  point.  The  proposed  sequence  of  events  is  to  have 
the  MURS  (at  position  1  on  Fig.  1 )  signal  the  AUV  (at  position 


2)  commanding  it  to  proceed  to  a  rendezvous  point  by  a 
certain  time.  The  AUV  computes  the  trajectory  and 
acknowledges  or  denies  the  command  (stage  A  on  Fig.l).  A 
denial  would  correspond  to  a  violation  of  some  constraint  with 
a  request  to  order  another  point  or  a  different  time  to 
rendezvous.  The  final  point  of  the  trajectory  is  located 
approximately  where  the  docking  station  would  be  located  on 
the  MURS  in  a  given  time.  Knowing  the  geometry  of  the 
MURS  allows  an  avoidance  area  to  be  constructed  that 
corresponds  to  the  aft  control  surfaces  and  the  screw.  The 
trajectory  needs  to  avoid  this  area.  Once  agreed,  both  the 
AUV  and  the  MURS  proceed  to  position  3  for  rendezvous 
(stage  B)  and  by  position  4  the  recovery  operation  (stage  C)  is 
complete. 


Once  again,  the  explored  rendezvous  scenario  assumes 
three  stages:  communication  (A),  execution  (B),  and  recovery 
(C),  respectively.  From  the  trajectory  generation  standpoint 
we  are  primarily  concerned  with  optimizing  the  path  that 
would  bring  the  AUV  from  its  current  position  (point  2)  to  a 
certain  rendezvous  state  (point  3)  in  the  preset  (handshaked 
with  the  MURS)  time  Tr,  while  obeying  all  possible  real-life 
constraints  and  avoiding  MURS  fins/screw  area. 

III.  Vehicle  Model 

The  Autonomous  Vehicles  Lab  at  the  Naval  Postgraduate 
School  operates  with  several  vehicles,  including  the  REMUS 
AUV.  The  REMUS  vehicle  is  commercially  produced  by 
Hydroid,  Inc  in  Pocasset,  MA  (www.hydroid.com).  A  variant 
of  the  REMUS  vehicle  is  currently  used  by  the  U.S.  Navy  for 
littoral  MIW.  It  has  a  proven  and  long  standing  employment 
history  within  the  U.S.  Navy  and  was  successfully  used  in 
support  of  Operation  Iraqi  Freedom  (OIF)  for  Mine 
Countermeasures  (MCM)  in  2002.  It  is  employed  by  several 
other  Navies  including  the  United  Kingdom,  Australia,  and 
Germany.  Since  it  is  commercially  produced,  many  of  the 
features  desired  by  the  Navy  are  either  already  available  or 
currently  in  development. 

There  are  three  model  options  for  the  REMUS  vehicle  itself, 
the  100,  600,  and  6000.  The  differences  are  mainly  size. 
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operating  depth,  speed,  and  sensor  packages.  The  model 
employed  in  OIF  and  owned  by  NPS  is  the  model  100.  This 
vehicle  is  small  and  perfectly  suited  for  many  operations.  It  is 
1.6m  (63 in)  long,  0.19m  (7.5/n)  in  diameter,  and  weighs  only 
36.3 kg  (80 lbs)  in  air.  It  has  an  operational  speed  of  1.54 m/s 
(3 kri)  allowing  22  hours  of  operation  time  or  8  hours  at  the 
maximum  speed  of  2.57 mis  (5kn).  The  maximum  operating 
depth  of  100m  (328 ft)  allows  it  to  be  invaluable  in  a  littoral 
environment. 

Woods  Hole  Oceanographic  Institute  has  developed  a 
prototype  docking  system  shown  in  Fig. 2.  REMUS  can  use 
Ultra  Short  Baseline  (USBL)  navigation  to  locate  and  transit 
to  a  docking  station  where  it  may  then  be  captured  and  secured. 
Once  the  vehicle  is  in  place,  a  connection  can  be  made  through 
which  data  may  be  transferred  to  or  from  the  vehicle,  while  its 
batteries  recharged. 


Fig.  2.  Prototype  of  REMUS  docking  station  [6,7], 


The  complete  6DoF  model  of  the  REMUS  AUV  to  support 
this  study  and  test  the  proposed  rendezvous  algorithms  has 
been  developed  [8,9].  It  accounts  for  standard  assumptions 
(vehicle  behaves  as  a  rigid  body,  the  Earth’s  rotation  is 
negligible,  all  of  the  forces  that  act  on  the  vehicle  have  either 
inertial  or  gravitational  origin)  and  include  linearized  dynamic 
differential  equations  for  surge  (m),  sway  (v),  heave  (w)  linear 
velocities,  and  roll  (;;),  pitch  ( q ),  yaw  (r)  angular  velocities. 
These  equations  are  fairly  common  and  their  development  is 
omitted  here.  These  dynamic  equations  are  augmented  with 
six  kinematic  equations.  Specifically,  with  three  equations  that 
relate  local  tangent  plane  (North-East-Down)  coordinates  of 
AUV’s  center  of  gravity  (x,  y  and  z)  to  the  components  of  the 
velocity  vector  expressed  in  the  body  frame  {/?}: 
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The  rotation  matrix  “hR  is  given  by 

cosy/  cos  6  -siny/  cosy/ sin  0 
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using  pitch  angle  6  and  yaw  angle  i//  (bank  angle  <p  is  small  and 
can  be  neglected).  Moreover,  for  simplicity  we  will  further 
reduce  the  matrix  (2)  to 
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assuming  relatively  small  pitch  angles  typical  for  AUVs 
(  <9  <30°  ).  Equation  (1)  implies  that  in  our  specific 
application  we  have  no  control  over  a  surge  component  of  the 
speed,  assuming  it  to  be  constant,  U0  .  In  case  the  currents  are 
known,  they  can  be  added  to  the  right-hand  side  of  equation 
(1)  to  get  proper  velocity  components  in  the  inertial  frame. 

Normally,  stabilization  would  be  the  first  priority  for 
developing  a  controller,  but  the  REMUS  vehicle  comes  with  a 
primary  controller  that  takes  care  of  it  already  installed.  This 
primary  controller  serves  several  functions  among  which  are 
stabilization  and  directly  controlling  the  control  surfaces  and 
propeller.  The  NPS  REMUS  vehicle  also  has  the  optional 
Remote  Control  Protocol  (RCP)  installed.  The  ASCII  text 
serial  protocol  permits  a  secondary  CPU  to  take  overriding 
control  of  the  vehicle.  This  is  normally  accomplished  via  the 
higher-level  inputs  sent  to  the  primary  controller.  These  inputs 
may  include  heading  T*  (or  yaw  rate  y/ ),  depth  z  (or  flight  path 
angle  y),  and  speed  (V).  This  secondary  controller  is  useful 
when  coupled  with  a  sensory  system  that  can  provide 
information  for  greater  autonomy  within  the  AUV.  An 
example  is  using  forward  looking  sonar  to  enable  reactive 
obstacle  avoidance  [9-12]. 

Summarizing,  for  the  purposes  of  this  paper  we  assume  that 
the  primary  and  secondary  controllers  discussed  above  are 
combined  into  one  autopilot  that  together  with  the  AUV  itself 
forms  a  stable  enhanced  plant  shown  on  Fig. 3.  By  making  this 
assumption,  we  remove  both  the  need  to  stabilize  the  system 
and  to  generate  actual  commands  for  the  control  surfaces  and 
rudder.  Instead,  to  control  the  vehicle  we  only  need  to 
generate  a  set  of  signals  consisting  of  yaw  rate,  flight  path 
angle,  and  speed  that  are  the  only  inputs  to  the 
autopilot-enhanced  system  necessary  for  navigating  from  one 
position  to  another.  (In  what  follows  we  even  reduce  a  set  of 
signals  to  only  two,  excluding  speed.) 


T  rajectory 

Inverse 

- ►  Yc  - 

- ► 

Enhanced 

Generator 

Dynamics 

^  /c 

— *■  K 

- ► 

Plant 

Fig.  3.  Incorporation  of  Trajectory  Generator  into  the  REMUS  control 
scheme. 


IV.  Rapid  Prototyping  of  Rendezvous  Trajectories 

The  proposed  real-time  trajectory  generator  block  shown  in 
Fig. 3  employs  the  modification  of  the  direct  method  of 
calculus  of  variations  originally  developed  for  aircraft  in  the 
mid  60 ’s  [13].  In  one  of  its  versions  the  entire  trajectory  was 
represented  as  a  combination  of  three  third-order  polynomials 
for  each  of  the  three  coordinates  (and  speed)  developed  in  the 
virtual  domain  of  some  abstract  argument  r  [14].  Later,  it  was 
shown  that  applying  higher-order  polynomials  allows 
satisfying  higher-order  derivatives  of  coordinates  at  the 
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terminal  points  making  it  possible  to  incorporate  this  approach 
onboard  the  vehicle  more  effectively  [15].  Married  together, 
the  two  approaches  ([14]  and  [15])  yield  a  very  effective  (from 
the  computational  standpoint)  and  robust  approach  for 
generating  feasible  ready-to-track  short-term  maneuvers  in 
real-time  [16,17].  This  latter  approach  has  already  been 
applied  to  generate  trajectories  for  different  vehicles  including 
AUVs  [10,18], 

Skipping  mathematics  that  has  been  already  addressed  for 
instance  in  [16]  and  [18],  it  can  be  noted  that  the  entire  3D 
trajectory  is  represented  as  three  seventh-order  polynomials, 
depending  on  the  vectors  of  preset  initial  and  final  boundary 
conditions  (including  up  to  the  second-order  derivative  of 
coordinates)  and  a  vector  of  varied  parameters  as  follows: 

x(T)  =  P(X0,Xf,il,T) 
y(T)  =  Py(X0,Xf,Sl,T)  (4) 

z(T)  =  pjx0,xf,n,T) 

(Note  that  these  polynomials  are  developed  in  the  virtual 
domain  and  not  in  the  time  domain,  allowing  for  independent 
optimization  of  the  speed  profile.) 

Vectors  X0  and  X  ,  are  composed  of:  a)  the  three 

components  of  the  current  initial  and  desired  final  position  of 
the  AUV,  b)  three  components  of  the  current  initial  and 
desired  final  velocity  of  the  AUV,  and  c)  the  three  components 
of  the  current  initial  and  desired  final  acceleration  of  the  AUV. 
Since  no  radical  maneuvers  at  the  terminal  point  are  desired, 
the  acceleration  components  at  the  final  point  (proportional  to 
the  second-order  derivatives  of  coordinates)  are  all  set  to  be 
zero. 

For  the  specific  problem  of  generation  the  AUV  rendezvous 
trajectories,  the  vector  of  varied  parameters  f>  includes  the 
value  of  the  arc  length  Tf  along  with  the  third-order 

derivatives  of  coordinates  at  the  terminal  points  (erk),  in  order 
to  adjust  the  trajectory  to  meet  all  constraints. 

As  shown  on  Fig. 3,  the  trajectory  generator  block  also 
includes  inversing  vehicle’s  dynamics  to  develop  necessary 
controls  and  account  for  all  constraints.  Once  the  candidate 
trajectory  (coefficients  of  polynomials  (4))  is  computed, 
inverse  dynamics  is  then  used  to  calculate  other  states  and  the 
required  controls  at  each  point  on  the  path.  The  values 
produced  by  the  trajectory  generation  algorithm  and  inverse 
dynamics  are  then  used  to  compute  the  performance  index  and 
estimate  the  degree  of  possible  violation  of  any  constraint. 
Using  these  data  the  varied  parameters  will  then  be  adjusted 
accordingly  to  achieve  the  minimum  of  the  performance  index 
while  satisfying  all  constraints. 

What  these  constraints  are?  For  any  vehicle,  the  control 
surfaces  can  only  move  so  far  and  it  can  only  go  so  fast. 
Keeping  in  mind  the  block-diagram  of  Fig. 3,  for  the  REMUS 
vehicle  this  translates  to  constraints  being  approximately 
equal  to  1 0'7,v  for  the  yaw  rate,  xjf ,  and  20°  for  flight  path 
angle,  y.  Since  there  is  an  area  that  we  do  not  want  the 
trajectory  to  go  through,  another  constraint  is  also  added,  so 


that  the  computed  trajectory  remains  outside  of  a  given 
volume.  This  volume  is  most  easily  represented  as  a  sphere 
with  a  radius  of  about  5  meters  and  having  its  center 
positioned,  so  that  the  entire  avoidance  area  is  contained 
within  it.  Finally,  and  not  immediately  obvious,  is  the  fact  that 
some  trajectories  may  attempt  to  take  the  vehicle  out  of  the 
water  or  below  the  sea  floor.  Since  neither  of  these  conditions 
is  feasible,  the  range  of  depth  allowed  for  the  trajectory  must 
be  limited. 

The  performance  index  may  have  several  components  with 
the  major  one  being  proportional  to  the  squared  difference 
between  the  computed  time  of  the  rendezvous  maneuver  tf  and 
predetermined  rendezvous  time  Tn  i.e.  (tf  —Tr  )2 .  Other  terms 

might  account  for  minimization  of  control  efforts  necessary  to 
bring  the  AUV  to  the  rendezvous  point,  for  instance 


tf 

y  \ 

2 

f 

¥(r)  taW1  'V/  'Vo 

+  ritf 

0 

l  *f-x oj 

Before  proceeding  with  the  derivation  of  inverse  dynamics, 
it  is  important  to  remember  that  the  trajectory  is  computed 
along  a  virtual  arc  and  not  in  the  time  domain.  This  means  that 
there  must  be  some  way  to  convert  from  the  virtual  arc 
domain,  r,  and  the  time  domain,  t.  This  conversion  is  given  by 


.  dr 
4  =  — , 

dt 


(5) 


where  1  is  the  so-called  speed  factor  [14].  The  discrete 
analogue  of  (5)  is 

Aj  =AzAt~1_l,  j  =  2,...,N,  (6) 

where 


AT  =  Tf(N-iy\  (7) 


and  N  is  the  number  of  increments  that  the  arc  length,  Tf  ,  is 
broken  into  during  the  numerical  procedure. 

Equation  (6)  indicates  that  X j  is  a  function  of  the  change  in 

r  divided  by  the  instantaneous  change  in  time.  The  subscript 
on  time  indicates  that  this  time  step  At  w  =  t j  - 1 j  ,  is  not 

constant  and  must  also  be  computed.  Intuitively,  this 
calculation  should,  and  does  involve  dividing  the  distance 
between  two  points  along  the  arc  by  the  speed: 

A.  _  yl(xj-xj-i)2+(yj-y]-i)2+(zj-zj_1)2  /on 

Ar  _ i  . - -  .  (o) 

po  +Vj_1+W2J_l 

Now  let  us  recall  our  system  dynamics  given  by  equation 
(1)  that  should  be  rewritten  in  the  virtual  domain  as 


xj 

~Uo 

4 

y'j 

=  lRW,) 

vj 

_wi_ 

In  order  to  compute  sway  and  heave  velocities  needed  in  (8) 
we  need  to  invert  system  (9)  with  respect  to  these  two  plus 
unknown  yaw  angle. 

We  can  represent  this  inversion  in  the  matrix  form  as 
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~U0~ 

cos  y/j 

sin  y/j 

0 

VJ 

-sin(i Vj 

cos  y/j 

0 

WJ. 

0 

0 

1 

(10) 


or  in  the  scalar  form  as 


U0  =  A(x'j  cos  y/j  +  y'j  sin  y/j ) , 

(11) 

Vj  =  Aj  (-x'j  sin  y/]  +  y]  cos  y/j ) , 

(12) 

JS 

ll 

(13) 

While  the  equation  (13)  is  readily  available  to  compute  the 
next  time  step  using  (8)  (the  derivatives  of  x,  y  and  z  are  given 
analytically  by  differentiating  equations  (4)  with  respect  to  r), 
equations  (11)-(12)  need  to  be  excluded  of  an  unknown  yaw 
angle  y/,  which  yields 

Vj=4W  +  y?)-Ul.  (14) 


Now,  in  order  to  begin  using  equations  (13)  and  (14)  in  (8) 
the  initial  value  of  A  must  be  assessed.  Since  equation  (5)  is 
literally  a  change  in  arc  length  per  unit  time,  it  is  feasible  to 
assume  that  the  initial  value  of  A  is  equal  to  the  initial  speed 
of  the  vehicle.  It  is  true  in  case  of  the  virtual  arc  length  Tf 

having  the  order  of  the  physical  path  length  sf  .  After  each 
iteration  we  may  readjust  it  according  to 

4=t/0  —  •  (15) 

sf 

Equations  (11)  and  (12)  can  also  be  resolved  to  yield  yj 

y/.=  tan-1  —  -  tan-1  — ,  (16) 

X]  uo 

and  therefore  a  command  yaw  rate 

(//.(/,  )-((//,-y/.  ,).V,  ..  (17) 

The  command  flight  path  angle  is  defined  as 

/ 

yc(tj)  =  taiT1  .  (18) 

7  \  '2  .  '2 

ixj  +yj 


V. Simulation  Results 

This  section  presents  an  example  of  the  trajectory 
generation  using  the  approach  discussed  above.  The  goal  is  to 
be  able  to  compute  a  rendezvous  trajectory  from  any  point  on 
the  AUV  holding  pattern  to  any  point  on  the  MURS  holding 
pattern  as  shown  on  Fig. 4  (for  stochastic  simulation  the 
circular  race  tracks  were  employed). 


Specifically,  using  the  scenario  stated  in  Section  II,  a 
MURS  is  moving  due  east  at  1  m/s  (1.94 kri)  with  the  docking 
station  at  a  depth  of  15m.  A  REMUS  vehicle  is  located  800 
meters  away.  The  MURS  wishes  to  conduct  rendezvous 
operation  Tr  minutes  later  and  sends  the  corresponding 
information  in  to  REMUS.  This  information  includes  the 
proposed  final  position,  xf,  yf,  Zf,  rendezvous  course,  speed  and 
time. 

With  the  optimization  procedure  an  initial  guess  is  made 
regarding  the  virtual  arc  length  of  the  trajectory  and  the 
required  components  of  the  initial  and  final  jerks.  It  takes  only 
a  few  seconds  (in  the  Mathworks  MATLAB/Simulink 
development  environment)  to  optimize  the  trajectory  to  the 
final  one  satisfying  all  constraints  and  reaching  the 
rendezvous  point  in  exactly  preset  time  (MATFAB’s 
fminsearch  function  was  used  to  minimize  the  performance 
index  augmented  with  weighted  penalties  for  constrains 
violation). 

Several  generated  trajectories  meeting  the  desired 
objectives  for  the  chosen  scenario  are  shown  in  Fig. 5  along 
with  an  obstacle  on  the  way  to  MURS  the  AUV  needs  to 
avoid.  These  trajectories  differ  by  the  arrival  time  Tr. 

While  handshaking  with  the  MURS,  the  AUV  determines 
whether  suggested  T,  is  feasible.  To  this  end,  among  four 
shown  trajectories  the  first  one,  with  T,=450.s,  happens  to  be 
infeasible  (the  constraints  on  controls  are  violated).  The 
solution  of  the  minimum-time  problem  for  this  scenario 
yielded  488.v  as  a  soonest  possible  rendezvous  time. 

Three  other  trajectories  on  Fig. 5  are  feasible.  That  means 
that  the  boundary  conditions  are  met  (by  construction)  and  all 
constraints  including  obstacle  avoidance  are  satisfied  (via 
optimization).  As  an  example,  Fig. 6  depicts  time  histories  of 
REMUS  vehicle’s  control  parameters  of  yaw  rate  y/c  and 
flight  path  angle  yc  for  the  trajectory  with  7',=600.v.  The  third 
plot  shows  the  speed  time  history. 


Fig.  5.  Examples  of  rendezvous  trajectories. 


The  stochastic  simulation  for  manifolds  shown  on  Fig. 4 
showed  that  in  all  of  those  cases  the  rendezvous  can  take  place 
if  Tr  is  greater  than  a  certain  value.  Furthermore,  they  show 
that  regardless  of  the  initial  guess  the  minimization  of  the 
performance  index  ensures  that  a  smooth,  realizable  trajectory 
is  calculated  just  in  a  few  seconds  (conversion  to  the 
executable  file  rather  than  using  interpretative  programming 
language  code  would  reduce  this  time  even  further). 
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Fig. 6.  Constrained  vehicle  parameters  for  7V=600s. 


VI.  Conclusion 

Based  on  the  simulations  it  can  be  stated  that  using  a  direct 
method  to  calculating  rendezvous  trajectories  results  in  a 
smooth,  realizable  path.  Constraints  can  be  inserted  to  ensure 
that  not  only  will  vehicle  parameters  not  be  violated,  but  that 
we  may  also  define  limits  on  the  trajectory  itself.  This 
demonstrates  and  supports  the  theoretical  feasibility  of  using 
the  direct  method  for  underwater  recovery  of  AUVs.  It  is 
expected  that  further  exploration  for  using  this  technique  will 
include  more  computer  simulations  incorporating  the  models 
of  primary  and  secondary  controllers  working  together  subject 
to  disturbances  and  experimentation  using  the  NPS  REMUS 
vehicle  at  the  Naval  Postgraduate  School. 
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